#!/usr/bin/python
# -*- coding: UTF-8 -*-


import time,threading,sys
sys.path.append("/home/biowin/Minibot/04.RobotSamples/03.SpiderRobot")
# sys.path.append("../../")
from BwRobotLib import BwRobotLib
from SpiderRobot import SpiderRobot
from BwSensorLib import BwSensorLib
import os,inspect

# 加载机器人构型配置
fileName = inspect.getframeinfo(inspect.currentframe()).filename
folderPath     = os.path.dirname(os.path.abspath(fileName))
cfile=folderPath+"/config.txt"


class LinePatrolRobot:
	def __init__(self):
		# 实例化传感器库对象
		self.sensorlib = BwsensorliblibLib("udp")
		# 实例化BwRobotLib库
		self.robotlib = BwRobotLib("udp")
		# 实例化蜘蛛机器人库
		self.spider = SpiderRobot(self.robotlib)
		# 连接机器人设备
		self.robotlib.connectRobot("10.10.100.254",8899) #真机AP模式
        # 导入机器人配置参数
        self.robotlib.loadConfig(config_file)
		#配置ID
		self.spider.bindIds(self.robotlib.module_ids)
		# 时间变量
		self.wait_time = 0.2
		# 点阵显示
		# 点阵图案：向上箭头 向下箭头　向左箭头　向右箭头　
		self.display_data = [[0x08,0x1C,0x2A,0x08,0x08],[0x08,0x08,0x2A,0x1C,0x08],
							[0x08,0x10,0x3F,0x10,0x08],[0x08,0x04,0x7E,0x04,0x08]]	

	
	def linePatrolRobotMotion(self):
		while 1:
			tracking = self.sensor.readTrackingSensor()  # 背面看传感器 得出数组元素分别为右传感器(ir1)、左传感器（ir2）
			time.sleep(0.1) 
			print(tracking)
			# ir1检测到黑色，ir2检测到黑色
			if (tracking[0] == 0 and tracking[1] == 0):  
				print("红外传感器未超限，巡线机器人向前进")
				self.sensorlib.setMatrixSensor(self.display_data[0])
				self.spider.goForward()
			# ir1检测到黑色，ir2检测到非黑色
			elif (tracking[0] == 0 and tracking[1] == 1):
				print("左红外传感器超限，巡线机器人向右转")
				self.sensorlib.setMatrixSensor(self.display_data[3])
				for a in range (0,1):
						self.spider.turnRight()
			# ir1检测到非黑色，ir2检测到黑色
			elif (tracking[0] == 1 and tracking[1] == 0):
				print("右红外传感器超限，巡线机器人向左转")
				self.sensorlib.setMatrixSensor(self.display_data[2])
				for a in range (0,1):
						self.spider.turnLeft()
			# ir1检测到非黑色，ir2检测到非黑色
			elif (tracking[0] == 1 and tracking[1] == 1):         
				print("左右红外传感器均超限，巡线机器人向后退")
				self.sensorlib.setMatrixSensor(self.display_data[1])
				self.spider.goBack()
			
			
# 主函数			
def main():
    # 实例化巡线机器人类
    linepatrol = LinePatrolRobot()
    # 执行巡线机器人功能
    linepatrol.linePatrolRobotMotion()


if __name__ == "__main__":
    try:
        main()
    except KeyboardInterrupt:
        print(" Program closed! ")
    pass

